perm filename PARALL.SAI[1,BGB] blob
sn#001250 filedate 1972-10-22 generic text, type T, neo UTF8
00100 ENTRY PARALLAX;
00200 BEGIN "PARALLAX"
00300 REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
00400 REQUIRE "TRIGER[SYS,BGB]" SOURCE_FILE;
00500 REQUIRE "COMMON[IA,BGB]" SOURCE_FILE;
00600
00700 α FEATURE DATUM - 3D LOCUS;
00800 REAL ARRAY ∂F[1:5]; α X,Y,Z,R,dR;
00900 REAL ARRAY ITEMVAR F;
01000 INTEGER ITEMVAR TV;
00100 α SKEW RETURNS X,Y,Z OF THE MIDPOINT OF THE SHORTEST LINE SEGMENT;
00200 α BETWEEN THE SKEW LINES OF SIGHT & D THE LENGTH OF THAT SEGMENT;
00300 PROCEDURE SKEW (REAL ARRAY ITEMVAR CAM1,CAM2,SWN1,SWN2; REAL ARRAY XYZD);
00400 BEGIN "SKEW"
00500 SAFE OWN REAL ARRAY C1,C2,R1,R2,S1,S2[1:3];
00600 REAL C11,C12,C21,C22,CC1,CC2,R12,X,Y,Z;
00700 DEFINE R11="10000",R22="10000",R21="R12";
00800 REAL SX,SY,K,K1,K2,R;
00900 INTEGER I;
01500 DEFINE DOT(X,Y)="X[1]*Y[1]+X[2]*Y[2]+X[3]*Y[3]";
01600 DEFINE THRICE="FOR I←1 STEP 1 UNTIL 3 DO";
01700 α GET THE LINE'O'SIGHT RAYS;
01800 SX ← PDX/LDX;
01900 SY ← PDY/LDY;
02000 X ← SX*∂(SWN1)[1];
02100 Y ← SY*∂(SWN1)[2];
02200 Z ← -FOCAL;
02300 THRICE R1[I]←∂(CAM1)[1,I]*X + ∂(CAM1)[2,I]*Y + ∂(CAM1)[3,I]*Z;
02400 X ← SX*∂(SWN2)[1];
02500 Y ← SY*∂(SWN2)[2];
02600 THRICE R2[I]←∂(CAM2)[1,I]*X + ∂(CAM2)[2,I]*Y + ∂(CAM2)[3,I]*Z;
02700 α NORMALIZE R VECTORS TO A HEALTHY 100 FEET;
02800 R ← 100/SQRT(R1[1]↑2 + R1[2]↑2 + R1[3]↑2);
02900 THRICE R1[I]← R1[I]*R;
03000 R ← 100/SQRT(R2[1]↑2 + R2[2]↑2 + R2[3]↑2);
03100 THRICE R2[I]← R2[I]*R;
03200 α PICK UP THE CAMERA LOCUS VECTORS;
03300 ARRBLT(C1[1],∂(CAM1)[4,1],3);
03400 ARRBLT(C2[1],∂(CAM2)[4,1],3);
00100 α DIAGONOSTIC OUTPUT:
00200 BEGIN
00300 REAL AZM,ALT,RXY:
00400 INTEGER I,J:
00500 OUTSTR(↓&"SKEW DIAGONOSTIC."&↓):
00600 OUTSTR("CAM1"&↓&9):
00700 SETFORMAT(10,3):
00800 FOR I←1 STEP 1 UNTIL 4 DO
00900 FOR J←1 STEP 1 UNTIL 3 DO
01000 OUTSTR(CVF(∂(CAM1)[I,J])&(IF J=3 THEN ↓&9 ELSE 9)):
01100 OUTSTR(↓&"CAM2"&↓&9):
01200 SETFORMAT(10,3):
01300 FOR I←1 STEP 1 UNTIL 4 DO
01400 FOR J←1 STEP 1 UNTIL 3 DO
01500 OUTSTR(CVF(∂(CAM2)[I,J])&(IF J=3 THEN ↓&9 ELSE 9)):
01600 OUTSTR(↓&"SWN1"&↓):
01700 FOR I←1 STEP 1 UNTIL 4 DO
01800 OUTSTR(9&CVS(∂(SWN1)[I])):
01900 OUTSTR(↓):
02000 OUTSTR(↓&"SWN2"&↓):
02100 FOR I←1 STEP 1 UNTIL 4 DO
02200 OUTSTR(9&CVS(∂(SWN2)[I])):
02300 OUTSTR(↓):
02400 SETFORMAT(10,4):
02500 OUTSTR(" SX = "&CVG(SX)&↓):
02600 OUTSTR(" SY = "&CVG(SY)&↓):
02700 OUTSTR("R1 = "):THRICE OUTSTR(CVG(R1[I])&9):OUTSTR(↓):
02800 AZM ← 180*ATAN2(R1[2],R1[1])/π:
02900 RXY ← SQRT(R1[2]↑2+R1[1]↑2):
03000 ALT ← 180*ATAN2(R1[3],RXY)/π:
03100 OUTSTR(9&"AZM = "&CVS(AZM)&9&"ALT = "&CVS(ALT)&↓):
03200 OUTSTR("R2 = "):THRICE OUTSTR(CVG(R2[I])&9):OUTSTR(↓):
03300 AZM ← 180*ATAN2(R2[2],R2[1])/π:
03400 RXY ← SQRT(R2[2]↑2+R2[1]↑2):
03500 ALT ← 180*ATAN2(R2[3],RXY)/π:
03600 OUTSTR(9&"AZM = "&CVS(AZM)&9&"ALT = "&CVS(ALT)&↓):
03700 OUTSTR("C1 = "):THRICE OUTSTR(CVG(C1[I])&9):OUTSTR(↓):
03800 OUTSTR("C2 = "):THRICE OUTSTR(CVG(C2[I])&9):OUTSTR(↓):
03900 SETFORMAT(0,7):
04000 INCHRW:
04100 END;
00100 α EVALUATE DOT SOME DOT PRODUCTS;
00200 C11 ← DOT(C1,R1);
00300 C12 ← DOT(C1,R2);
00400 C21 ← DOT(C2,R1);
00500 C22 ← DOT(C2,R2);
00600 R12 ← DOT(R1,R2);
00700 α TWO MORE CONSTANTS;
00800 CC1 ← C21-C11;
00900 CC2 ← C22-C12;
01000 α USE KRAMER'S RULE TO SOLVE FOR THE PARAMETRIC VARIABLES;
01100 K ← R12*R21 - R11*R22;
01200 K1 ← (CC2*R21 - CC1*R22)/K;
01300 K2 ← (R11*CC2 - R12*CC1)/K;
01400 α GET THE ENDPOINTS OF THE TRANSVERSAL SEGMENT;
01500 THRICE S1[I] ← C1[I] + K1*R1[I];
01600 THRICE S2[I] ← C2[I] + K2*R2[I];
01700 α RETURN THE TRANSVERSALS MIDPOINT AND LENGTH;
01800 THRICE XYZD[I] ← (S1[I]+S2[I])/2;
01900 XYZD[4]←SQRT((S1[1]-S2[1])↑2 +(S1[2]-S2[2])↑2 +(S1[3]-S2[3])↑2);
02000 α DIAGONOSTIC PRINT OUT:
02100 OUTSTR("S1 = "):THRICE OUTSTR(CVG(S1[I])&9):OUTSTR(↓):
02200 OUTSTR("S2 = "):THRICE OUTSTR(CVG(S2[I])&9):OUTSTR(↓):
02300 OUTSTR("XYZ= "):THRICE OUTSTR(CVG(XYZD[I])&9):OUTSTR(↓):
02400 OUTSTR("D = "):OUTSTR(CVG(XYZD[4])&↓):
02500 INCHRW;
02600 END "SKEW";
00100 α FEATURE LOCUS SOLVER BY PARALLAX;
00200 INTERNAL PROCEDURE PARALLAX;
00300 BEGIN "PARALLAX"
00400 SET TMPSET;
00500 INTEGER I,J,K,M,N,VIEWS,WORST,FLG;
00600 REAL ARRAY XYZD[1:4];
00700 REAL X,Y,Z,R,RMAX,RSUM,RMEAN;
00800 ITEMVAR DUM1,DUM2;
00900 BEGIN "GETF"
01000 STRING STR; LABEL L;
01100 α STROBE THE USER FOR A FEATURE NAME;
01200 L: OUTSTR(↓&9&"FEATURE = ");
01300 STR ← INCHWL;
01400 F ← CVSI(STR,FLG);
01500 IF FLG THEN GO L;
01600 END "GETF";
01700 α COLLECT THE TVFILES THAT CONTAIN THIS FEATURE;
01800 TMPSET←PHI;
01900 ∀ TV,DUM1,DUM2|TV⊗F≡DUM1∧LOCOR⊗TV≡DUM2 DO PUT TV IN TMPSET;
02000 VIEWS ← LENGTH(TMPSET);
02100 IF VIEWS=0 THEN
02200 BEGIN OUTSTR("NO VIEWS"&↓&"*");RETURN;END;
02300 OUTSTR(CVS(VIEWS)&" VIEWS."&↓);
02400 N ← VIEWS;
02500 M ← N*(N-1)%2;
00100 α ALLOCATE WORKING ARRAYS;
00200 BEGIN "BLK2"
00300 REAL ARRAY D[1:N,1:N]; α DISTANCE BETWEEN RAYS;
00400 REAL ARRAY RAZALT[1:N,1:4];
00500 ITEMVAR ARRAY TVS,SWN,CAM[1:N];
00600 REAL ARRAY XYZ[1:M,1:3];
00700 α STASH EVERYTHING INTO THE WORKING ARRAYS;
00800 FOR I←1 STEP 1 UNTIL N DO
00900 BEGIN
01000 SET Q;
01100 IF LENGTH(TMPSET)=0 THEN OUTSTR("TMPSET FUCKED UP."&↓);
01200 TV ← LOP(TMPSET);
01300 TVS[I] ← TV;
01400 Q←TV⊗F; IF LENGTH(Q)=0 THEN OUTSTR("TV⊗F FUCKED UP."&↓);
01500 SWN[I] ← LOP(Q);
01600 Q←LOCOR⊗TV; IF LENGTH(Q)=0 THEN OUTSTR("LOCOR⊗TV FUCKED UP."&↓)ELSE
01700 CAM[I] ← LOP(Q);
01800 END;
00100 α SKEW EACH TV RAY TO THE FEATURE INTO ALL THE OTHERS;
00200 WHILE TRUE DO
00300 BEGIN "BLK3"
00400 K ← 1;
00500 X←Y←Z ← 0;
00600 FOR I←1 STEP 1 UNTIL N-1 DO
00700 FOR J←I+1 STEP 1 UNTIL N DO
00800 BEGIN
00900 SKEW(CAM[I],CAM[J],SWN[I],SWN[J],XYZD);
01000 D[I,J]←D[J,I]←XYZD[4];
01100 ARRBLT(XYZ[K,1],XYZD[1],3);
01200 K ← K+1;
01300 X←X+XYZD[1]; Y←Y+XYZD[2]; Z←Z+XYZD[3];
01400 END;
01500 α AVERAGE THE FEATURE LOCII;
01600 X←X/M; Y←Y/M; Z←Z/M;
01700 α COMPUTE THE MEAN AND MAX RADIUS OF FEATURE LOCII;
01800 RMAX ← RSUM ← 0;
01900 FOR K←1 STEP 1 UNTIL M DO
02000 BEGIN
02100 R ← SQRT((XYZ[K,1]-X)↑2 +(XYZ[K,2]-Y)↑2 +(XYZ[K,3]-Z)↑2);
02200 RMAX ← R MAX RMAX;
02300 RSUM ← RSUM + R;
02400 END;
02500 RMEAN ← RSUM/M;
00100 α OUTPUT THE FEATURE LOCUS SOLUTION;
00200 OUTSTR(↓);
00300 OUTSTR(" ");
00400 OUTSTR(" X");
00500 OUTSTR(" Y");
00600 OUTSTR(" Z");
00700 OUTSTR(" RMAX");
00800 OUTSTR(" RMEAN");
00900 OUTSTR(↓);
01000 OUTSTR("FEATURE LOCUS ");
01100 SETFORMAT(10,3);
01200 OUTSTR(CVG(X));
01300 OUTSTR(CVG(Y));
01400 OUTSTR(CVG(Z));
01500 OUTSTR(CVG(RMAX));
01600 OUTSTR(CVG(RMEAN));
01700 OUTSTR(↓&↓);
01800 α COMPUTE THE AVERAGE TRANSVERAL LENGTHS FROM EACH CAMERA TO THE OTHERS;
01900 FOR I←1 STEP 1 UNTIL N DO
02000 BEGIN
02100 REAL SUM;
02200 SUM ← 0;
02300 FOR J←1 STEP 1 UNTIL N DO
02400 IF J≠I THEN SUM←SUM+D[I,J];
02500 D[I,I] ← SUM/(N-1);
02600 END;
00100 α OUTPUT THE RAZALT IN ORDER ON MEAN D BEST TO WORST;
00200 OUTSTR("TVFILE DMEAN RANGE AZIMUTH ALTITUDE"&↓);
00300 WHILE TRUE DO
00400 BEGIN
00500 REAL BEST,DX,DY,DZ,RANGE,AZM,ALT,RXY;
00600 REAL ARRAY ITEMVAR C;
00700 WORST ← J;
00800 J ← 0; BEST ← 999999;
00900 FOR I←1 STEP 1 UNTIL N DO IF D[I,I]<BEST THEN
01000 BEGIN BEST←D[I,I];J←I;END;
01100 IF J=0 THEN DONE;
01200 D[J,J] ← 1000000;
01300 OUTSTR(CVIS(TVS[J],FLG)&9);
01400 OUTSTR(CVG(BEST));
01500 C ← CAM[J];
01600 DX ← X - ∂(C)[4,1];
01700 DY ← Y - ∂(C)[4,2];
01800 DZ ← Z - ∂(C)[4,3];
01900 RANGE ← SQRT(DX↑2+DY↑2+DZ↑2);
02000 RXY ← SQRT(DX↑2+DY↑2);
02100 AZM ← 180*ATAN2(DY,DX)/π;
02200 ALT ← 180*ATAN2(DZ,RXY)/π;
02300 OUTSTR(CVG(RANGE));
02400 OUTSTR(CVG(AZM));
02500 OUTSTR(CVG(ALT));
02600 OUTSTR(↓);
02700 END;
00100 α SWITCH OPTIONS;
00200 BEGIN "SWITCH"
00300 INTEGER CHR;
00400 LABEL L;
00500 L: OUTSTR(↓&"+,-,Y,N,R SWITCH ?");
00600 CHR ← INCHRW;
00700 IF CHR="Y" THEN
00800 BEGIN
00900 ∂(F)[1] ← X;
01000 ∂(F)[2] ← Y;
01100 ∂(F)[3] ← Z;
01200 ∂(F)[5] ← RMEAN;
01300 OUTSTR(↓&"*");
01400 RETURN;
01500 END ELSE
01600 IF CHR="N" THEN BEGIN OUTSTR(↓&"*");RETURN;END ELSE
01700 IF CHR="+" ∧ N≠VIEWS THEN N←N+1 ELSE
01800 IF CHR="-" ∧ N≠2 THEN
01900 BEGIN
02000 TVS[WORST]↔TVS[N];
02100 SWN[WORST]↔SWN[N];
02200 CAM[WORST]↔CAM[N];
02300 N ← N-1;
02400 END ELSE GO L;
02500 M ← N*(N-1)%2;
02600 END "SWITCH";
02700 END "BLK3";
02800 END "BLK2";
02900 END "PARALLAX";
03000
03100 END "PARALLAX";